import socket
import time
import threading

class RTKDriver:
    '''
        基于UDP的RTK接收端
    '''

    def __init__(self, port=8890, buffer_len=200,callback=None):
        """
            初始化UDP接收端
        """
        self.port = port
        self.buffer = {}
        self.buffer_len=buffer_len
        self.socket = None
        self.callback=callback
        self.Start()

    def Start(self):
        """启动UDP接收端"""
        try:
            self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
            self.socket.bind(('', self.port))
            print(f"UDP接收端{self.port}已启动!")
            t=threading.Thread(target=self._receive_data,args=())
            t.setDaemon(True)
            t.start()
        except Exception as e:
            print(f"启动UDP端口出错: {e}")

    def _receive_data(self):
        """内部方法：接收并处理数据"""
        while True:
            # 接收数据
            line, address = self.socket.recvfrom(1024)
            line = line.decode('utf-8').strip()
            if not (line.startswith('INFO') and line.endswith('RTCMDATA')): continue
            line = line[0:line.find('MESSAGE')]
            target = {"IP": "", "MODE": "", "UTC": "", "RSSI": "", "RMS": "", "LLA": [0.0] * 3, "XYZ": [0.0] * 3,
                      "MSG": "","DIR":0.0,"SPD":0.0, "DELAY": 0.0, "STAMP": 0.0}
            parts = line.split(',')
            for part in parts:
                if part.startswith('IP'):
                    target['IP'] = part.strip('IP')
                elif part.startswith('UTC'):
                    target['UTC'] = part.strip('UTC')
                elif part.startswith('MODE'):
                    target['MODE'] = part.strip('MODE')
                elif part.startswith('RSSI'):
                    target['RSSI'] = int(part.strip('RSSI'))
                elif part.startswith('RMS'):
                    target['RMS'] = float(part.strip('RMS'))
                elif part.startswith('DIR'):
                    target['DIR'] = float(part.strip('DIR'))
                elif part.startswith('SPD'):
                    target['SPD'] = float(part.strip('SPD'))    
                elif part.startswith('GPSX'):
                    target['LLA'][0] = float(part.strip('GPSX')) / 100000000.0
                elif part.startswith('GPSY'):
                    target['LLA'][1] = float(part.strip('GPSY')) / 100000000.0
                elif part.startswith('GPSZ'):
                    target['LLA'][2] = float(part.strip('GPSZ'))
                elif part.startswith('X'):
                    target['XYZ'][0] = float(part.strip('X'))
                elif part.startswith('Y'):
                    target['XYZ'][1] = float(part.strip('Y'))
                elif part.startswith('Z'):
                    target['XYZ'][2] = float(part.strip('Z'))
            target['STAMP'] = time.time()
            uid = parts[2].strip('UID')
            if not uid in self.buffer: self.buffer[uid] = []
            self.buffer[uid].append(target)
            if len(self.buffer[uid]) > 1: self.buffer[uid][-1]['DELAY'] = self.buffer[uid][-1]['STAMP'] - \
                                                                          self.buffer[uid][-2]['STAMP']
            if len(self.buffer[uid]) > self.buffer_len: self.buffer[uid].pop(0)
            if self.callback: self.callback()


    def Stop(self):
        """停止UDP接收端并释放资源"""
        if self.socket:
            self.socket.close()
            self.socket = None
            print("接收端已停止")


if __name__ == "__main__":
    # 创建并启动UDP接收端
    receiver = RTKDriver(port=8890,debug=False)
    while True: time.sleep(1)